#include "perception_visualizer.h"

#include <pcl/common/common.h>

#include <boost/random.hpp>

#include "common/utility.h"

namespace autodrive {
namespace perception {

PerceptionVisualizer::PerceptionVisualizer() {
  is_init_ = false;
  is_dispose_ = false;
}

PerceptionVisualizer::~PerceptionVisualizer() { is_dispose_ = true; }

bool PerceptionVisualizer::Init(boost::json::object& viewer_json_root) noexcept(
    false) {
  if (is_init_) {
    return true;
  }

  // set viewer style
  if (!set_viewer_style(viewer_json_root)) {
    return false;
  }

  // start viewer task
  auto viewer_task_func{std::bind(&PerceptionVisualizer::visualize, this)};
  viewer_task_main_ = std::thread(viewer_task_func);

  // start pcl viewer task
  auto pcl_viewer_task_func{
      std::bind(&PerceptionVisualizer::pcl_visualize, this)};
  pcl_viewer_task_ = std::thread(pcl_viewer_task_func);

  // viewer point cloud
  point_cloud_ptr_ = pcl::make_shared<pcl::PointCloud<pcl::PointXYZ>>();

  is_init_ = true;
  return true;
}

PerceptionVisualizer& PerceptionVisualizer::GetInstance() noexcept(false) {
  static PerceptionVisualizer instance;
  return instance;
}

void PerceptionVisualizer::Enqueue(std::string timestamp, Map const* src_data,
                                   Map const* filter_data,
                                   Map const* truth_data) noexcept(false) {
  if (!viewer_buffer_.IsFull()) {
    ViewerBufferStruct buffer_data;
    buffer_data.timestamp = timestamp;
    buffer_data.map_src_ = src_data;
    buffer_data.map_filter_ = filter_data;
    buffer_data.map_truth_ = truth_data;
    viewer_buffer_.Push(buffer_data);
  }
}

void PerceptionVisualizer::visualize(
    PerceptionVisualizer* const perceptionVisualizer) noexcept(false) {
  while (true) {
    {
      std::unique_lock<std::shared_mutex> lock(perceptionVisualizer->mutex_);
      if (!perceptionVisualizer->viewer_buffer_.IsEmpty()) {
        perceptionVisualizer->current_data_ =
            perceptionVisualizer->viewer_buffer_.Pop();
        perceptionVisualizer->pcl_redraw_ = true;
        perceptionVisualizer->not_redraw_condition_.wait(
            lock, [&]() { return !perceptionVisualizer->pcl_redraw_; });
      } else {
        std::chrono::milliseconds dura(10);
        std::this_thread::sleep_for(dura);
      }
    }
  }
}

void PerceptionVisualizer::pcl_visualize(
    PerceptionVisualizer* const perceptionVisualizer) noexcept(false) {
  auto visualizer_deleter = [](pcl::visualization::PCLVisualizer* visualizer) {
    if (visualizer != nullptr) {
      visualizer->close();
    }
  };

  auto pcl_viewer_ptr{pcl::visualization::PCLVisualizer::Ptr(
      new pcl::visualization::PCLVisualizer(), visualizer_deleter)};

  pcl_viewer_ptr->createViewPort(0.0, 0.5, 0.5, 1.0,
                                 perceptionVisualizer->pcl_viewer_src_);
  pcl_viewer_ptr->createViewPort(0.0, 0.0, 0.5, 0.5,
                                 perceptionVisualizer->pcl_viewer_filter_);
  pcl_viewer_ptr->createViewPort(0.5, 0.5, 1.0, 1.0,
                                 perceptionVisualizer->pcl_viewer_truth_);

  pcl_viewer_ptr->createViewPort(0.5, 0.0, 1.0, 0.5,
                                 perceptionVisualizer->pcl_viewer_info_);

  // 设置缩放
  pcl_viewer_ptr->setCameraPosition(
      perceptionVisualizer->viewport_zoom_center_.x,
      perceptionVisualizer->viewport_zoom_center_.y,
      perceptionVisualizer->viewport_zoom_center_.z +
          perceptionVisualizer->viewport_zoom_distance_,
      perceptionVisualizer->viewport_zoom_center_.x,
      perceptionVisualizer->viewport_zoom_center_.y, 0, 0, 0, 0);
  perceptionVisualizer->viewport_zoom_distance_ = 0;

  try {
    while (true) {
      if (perceptionVisualizer->is_dispose_) {
        return;
      }

      ViewerBufferStruct current_data{perceptionVisualizer->current_data_};
      auto isRedraw{perceptionVisualizer->pcl_redraw_};
      if (!isRedraw) {
        pcl_viewer_ptr->spinOnce();
        std::chrono::milliseconds dura(10);
        std::this_thread::sleep_for(dura);
      } else {
        std::shared_lock<std::shared_mutex> lock(perceptionVisualizer->mutex_);

        pcl_viewer_ptr->removeAllCoordinateSystems();
        pcl_viewer_ptr->removeAllPointClouds();
        pcl_viewer_ptr->removeAllShapes();

        if (current_data.map_src_ == nullptr ||
            current_data.map_filter_ == nullptr ||
            current_data.map_truth_ == nullptr) {
          perceptionVisualizer->pcl_redraw_ = false;
          perceptionVisualizer->not_redraw_condition_.notify_one();
          pcl_viewer_ptr->spinOnce();
          continue;
        }
        // 坐标系
        pcl_viewer_ptr->addCoordinateSystem();

        // 显示lane_dividers
        perceptionVisualizer->display_lane_divider(
            pcl_viewer_ptr, *current_data.map_src_,
            perceptionVisualizer->lane_divider_color_.r,
            perceptionVisualizer->lane_divider_color_.g,
            perceptionVisualizer->lane_divider_color_.b,
            perceptionVisualizer->pcl_viewer_src_, false);

        perceptionVisualizer->display_lane_divider(
            pcl_viewer_ptr, *current_data.map_filter_,
            perceptionVisualizer->lane_divider_color_.r,
            perceptionVisualizer->lane_divider_color_.g,
            perceptionVisualizer->lane_divider_color_.b,
            perceptionVisualizer->pcl_viewer_filter_,
            perceptionVisualizer->filter_random_color_flag_);

        perceptionVisualizer->display_lane_divider(
            pcl_viewer_ptr, *current_data.map_truth_,
            perceptionVisualizer->lane_divider_color_.r,
            perceptionVisualizer->lane_divider_color_.g,
            perceptionVisualizer->lane_divider_color_.b,
            perceptionVisualizer->pcl_viewer_truth_, false);

        // 显示road_edges
        perceptionVisualizer->display_road_edge(
            pcl_viewer_ptr, *current_data.map_src_,
            perceptionVisualizer->road_edges_color_.r,
            perceptionVisualizer->road_edges_color_.g,
            perceptionVisualizer->road_edges_color_.b,
            perceptionVisualizer->pcl_viewer_src_, false);

        perceptionVisualizer->display_road_edge(
            pcl_viewer_ptr, *current_data.map_filter_,
            perceptionVisualizer->road_edges_color_.r,
            perceptionVisualizer->road_edges_color_.g,
            perceptionVisualizer->road_edges_color_.b,
            perceptionVisualizer->pcl_viewer_filter_,
            perceptionVisualizer->filter_random_color_flag_);

        perceptionVisualizer->display_road_edge(
            pcl_viewer_ptr, *current_data.map_truth_,
            perceptionVisualizer->road_edges_color_.r,
            perceptionVisualizer->road_edges_color_.g,
            perceptionVisualizer->road_edges_color_.b,
            perceptionVisualizer->pcl_viewer_truth_, false);

        // 显示lane_divider_joints
        perceptionVisualizer->display_lane_divider_joints(
            pcl_viewer_ptr, *current_data.map_src_,
            perceptionVisualizer->lane_divider_joints_color_.r,
            perceptionVisualizer->lane_divider_joints_color_.g,
            perceptionVisualizer->lane_divider_joints_color_.b,
            perceptionVisualizer->pcl_viewer_src_, false);

        perceptionVisualizer->display_lane_divider_joints(
            pcl_viewer_ptr, *current_data.map_filter_,
            perceptionVisualizer->lane_divider_joints_color_.r,
            perceptionVisualizer->lane_divider_joints_color_.g,
            perceptionVisualizer->lane_divider_joints_color_.b,
            perceptionVisualizer->pcl_viewer_filter_,
            perceptionVisualizer->filter_random_color_flag_);

        perceptionVisualizer->display_lane_divider_joints(
            pcl_viewer_ptr, *current_data.map_truth_,
            perceptionVisualizer->lane_divider_joints_color_.r,
            perceptionVisualizer->lane_divider_joints_color_.g,
            perceptionVisualizer->lane_divider_joints_color_.b,
            perceptionVisualizer->pcl_viewer_truth_, false);

        // 显示center_lines
        perceptionVisualizer->display_center_lines(
            pcl_viewer_ptr, *current_data.map_src_,
            perceptionVisualizer->center_lines_color_.r,
            perceptionVisualizer->center_lines_color_.g,
            perceptionVisualizer->center_lines_color_.b,
            perceptionVisualizer->pcl_viewer_src_, false);

        perceptionVisualizer->display_center_lines(
            pcl_viewer_ptr, *current_data.map_filter_,
            perceptionVisualizer->center_lines_color_.r,
            perceptionVisualizer->center_lines_color_.g,
            perceptionVisualizer->center_lines_color_.b,
            perceptionVisualizer->pcl_viewer_filter_,
            perceptionVisualizer->filter_random_color_flag_);

        perceptionVisualizer->display_center_lines(
            pcl_viewer_ptr, *current_data.map_truth_,
            perceptionVisualizer->center_lines_color_.r,
            perceptionVisualizer->center_lines_color_.g,
            perceptionVisualizer->center_lines_color_.b,
            perceptionVisualizer->pcl_viewer_truth_, false);

        // 显示road_markings
        perceptionVisualizer->display_road_markings(
            pcl_viewer_ptr, *current_data.map_src_,
            perceptionVisualizer->road_markings_color_.r,
            perceptionVisualizer->road_markings_color_.g,
            perceptionVisualizer->road_markings_color_.b,
            perceptionVisualizer->pcl_viewer_src_, false);

        perceptionVisualizer->display_road_markings(
            pcl_viewer_ptr, *current_data.map_filter_,
            perceptionVisualizer->road_markings_color_.r,
            perceptionVisualizer->road_markings_color_.g,
            perceptionVisualizer->road_markings_color_.b,
            perceptionVisualizer->pcl_viewer_filter_,
            perceptionVisualizer->filter_random_color_flag_);

        perceptionVisualizer->display_road_markings(
            pcl_viewer_ptr, *current_data.map_truth_,
            perceptionVisualizer->road_markings_color_.r,
            perceptionVisualizer->road_markings_color_.g,
            perceptionVisualizer->road_markings_color_.b,
            perceptionVisualizer->pcl_viewer_truth_, false);

        // 显示自车
        perceptionVisualizer->point_cloud_ptr_->clear();
        perceptionVisualizer->point_cloud_ptr_->push_back({0, 0, 0});
        perceptionVisualizer->base_display_cloud(
            pcl_viewer_ptr, perceptionVisualizer->point_cloud_ptr_,
            perceptionVisualizer->viewer_car_size_,
            perceptionVisualizer->car_color_.r,
            perceptionVisualizer->car_color_.g,
            perceptionVisualizer->car_color_.b, "car point",
            perceptionVisualizer->pcl_viewer_src_);

        // 显示timestamp
        pcl_viewer_ptr->addText(current_data.timestamp, 0, 0,
                                perceptionVisualizer->viewer_text_size_, 1, 1,
                                1, "timestamp",
                                perceptionVisualizer->pcl_viewer_info_);

        perceptionVisualizer->pcl_redraw_ = false;
        perceptionVisualizer->not_redraw_condition_.notify_one();
        pcl_viewer_ptr->spinOnce();
      }
    }
  } catch (const std::exception& e) {
    std::cerr << "[ " << to_red("ERROR") << "] PerceptionVisualizer: "
              << "pcl visualize error: ";
    std::cerr << e.what() << std::endl;
  } catch (...) {
    std::cerr << "[ " << to_red("ERROR") << "] PerceptionVisualizer: "
              << "pcl visualize unexpected error!" << std::endl;
  }
}

bool PerceptionVisualizer::set_viewer_style(
    boost::json::object& viewer_json_root) noexcept(false) {
  // get zoom scale conf
  if (!viewer_json_root.contains("Zoom_Scale")) {
    std::cerr << autodrive::perception::to_red("[Error] ")
              << "Config json content does not contain the Zoom_Scale !"
              << std::endl;
    return false;
  }
  viewport_zoom_distance_scale_ = viewer_json_root["Zoom_Scale"].as_double();

  // get draw line flag
  if (!viewer_json_root.contains("Draw_Line")) {
    std::cerr << autodrive::perception::to_red("[Error] ")
              << "Config json content does not contain the Draw_Line !"
              << std::endl;
    return false;
  }
  is_draw_line_ = viewer_json_root["Draw_Line"].as_bool();

  // get draw polygon flag
  if (!viewer_json_root.contains("Draw_Polygon")) {
    std::cerr << autodrive::perception::to_red("[Error] ")
              << "Config json content does not contain the Draw_Polygon !"
              << std::endl;
    return false;
  }
  is_draw_polygon_ = viewer_json_root["Draw_Polygon"].as_bool();

  // get point size
  if (!viewer_json_root.contains("Point_Size")) {
    std::cerr << autodrive::perception::to_red("[Error] ")
              << "Config json content does not contain the Point_Size !"
              << std::endl;
    return false;
  }
  viewer_point_size_ = viewer_json_root["Point_Size"].as_int64();
  viewer_keypoint_size_ = viewer_json_root["Keypoint_Size"].as_int64();

  // get text size
  if (!viewer_json_root.contains("Text_Size")) {
    std::cerr << autodrive::perception::to_red("[Error] ")
              << "Config json content does not contain the Text_Size !"
              << std::endl;
    return false;
  }
  viewer_text_size_ = viewer_json_root["Text_Size"].as_int64();

  // get car size
  if (!viewer_json_root.contains("Car_Size")) {
    std::cerr << autodrive::perception::to_red("[Error] ")
              << "Config json content does not contain the Car_Size !"
              << std::endl;
    return false;
  }
  viewer_car_size_ = viewer_json_root["Car_Size"].as_int64();

  // get filter_random_color
  if (!viewer_json_root.contains("Filter_Random_Color")) {
    std::cerr
        << autodrive::perception::to_red("[Error] ")
        << "Config json content does not contain the Filter_Random_Color !"
        << std::endl;
    return false;
  }
  filter_random_color_flag_ = viewer_json_root["Filter_Random_Color"].as_bool();

  // get View_X_Range
  if (!viewer_json_root.contains("View_X_Range")) {
    std::cerr << autodrive::perception::to_red("[Error] ")
              << "Config json content does not contain the View_X_Range !"
              << std::endl;
    return false;
  }
  boost::json::array view_x_range = viewer_json_root["View_X_Range"].as_array();
  if (view_x_range.size() != 2) {
    std::cerr << autodrive::perception::to_red("[Error] ")
              << "Config json exception, get View_X_Range failed !"
              << std::endl;
    return false;
  }
  viewport_x_min_ = view_x_range[0].as_double();
  viewport_x_max_ = view_x_range[1].as_double();

  // get View_Y_Range
  if (!viewer_json_root.contains("View_Y_Range")) {
    std::cerr << autodrive::perception::to_red("[Error] ")
              << "Config json content does not contain the View_Y_Range !"
              << std::endl;
    return false;
  }
  boost::json::array view_y_range = viewer_json_root["View_Y_Range"].as_array();
  if (view_y_range.size() != 2) {
    std::cerr << autodrive::perception::to_red("[Error] ")
              << "Config json exception, get View_Y_Range failed !"
              << std::endl;
    return false;
  }
  viewport_y_min_ = view_y_range[0].as_double();
  viewport_y_max_ = view_y_range[1].as_double();

  // get Lane_Divider_Color
  if (!viewer_json_root.contains("Lane_Divider_Color")) {
    std::cerr << autodrive::perception::to_red("[Error] ")
              << "Config json content does not contain the Lane_Divider_Color !"
              << std::endl;
    return false;
  }
  boost::json::array lane_divider_color =
      viewer_json_root["Lane_Divider_Color"].as_array();
  if (lane_divider_color.size() != 3) {
    std::cerr << autodrive::perception::to_red("[Error] ")
              << "Config json exception, get Lane_Divider_Color failed !"
              << std::endl;
    return false;
  }
  lane_divider_color_ = {lane_divider_color[0].as_double(),
                         lane_divider_color[1].as_double(),
                         lane_divider_color[2].as_double()};

  // get Lane_Divider_Joints_Color
  if (!viewer_json_root.contains("Lane_Divider_Joints_Color")) {
    std::cerr << autodrive::perception::to_red("[Error] ")
              << "Config json content does not contain the "
                 "Lane_Divider_Joints_Color !"
              << std::endl;
    return false;
  }
  boost::json::array lane_divider_joints_color =
      viewer_json_root["Lane_Divider_Joints_Color"].as_array();
  if (lane_divider_joints_color.size() != 3) {
    std::cerr << autodrive::perception::to_red("[Error] ")
              << "Config json exception, get Lane_Divider_Joints_Color failed !"
              << std::endl;
    return false;
  }
  lane_divider_joints_color_ = {lane_divider_joints_color[0].as_double(),
                                lane_divider_joints_color[1].as_double(),
                                lane_divider_joints_color[2].as_double()};

  // get Road_Edges_Color
  if (!viewer_json_root.contains("Road_Edges_Color")) {
    std::cerr << autodrive::perception::to_red("[Error] ")
              << "Config json content does not contain the "
                 "Road_Edges_Color !"
              << std::endl;
    return false;
  }
  boost::json::array road_edges_color =
      viewer_json_root["Road_Edges_Color"].as_array();
  if (road_edges_color.size() != 3) {
    std::cerr << autodrive::perception::to_red("[Error] ")
              << "Config json exception, get Road_Edges_Color failed !"
              << std::endl;
    return false;
  }
  road_edges_color_ = {road_edges_color[0].as_double(),
                       road_edges_color[1].as_double(),
                       road_edges_color[2].as_double()};

  // get Center_Lines_Color
  if (!viewer_json_root.contains("Center_Lines_Color")) {
    std::cerr << autodrive::perception::to_red("[Error] ")
              << "Config json content does not contain the "
                 "Center_Lines_Color !"
              << std::endl;
    return false;
  }
  boost::json::array center_lines_color =
      viewer_json_root["Center_Lines_Color"].as_array();
  if (center_lines_color.size() != 3) {
    std::cerr << autodrive::perception::to_red("[Error] ")
              << "Config json exception, get Center_Lines_Color failed !"
              << std::endl;
    return false;
  }
  center_lines_color_ = {center_lines_color[0].as_double(),
                         center_lines_color[1].as_double(),
                         center_lines_color[2].as_double()};

  // get Road_Markings_Color
  if (!viewer_json_root.contains("Road_Markings_Color")) {
    std::cerr << autodrive::perception::to_red("[Error] ")
              << "Config json content does not contain the "
                 "Road_Markings_Color !"
              << std::endl;
    return false;
  }
  boost::json::array road_markings_color =
      viewer_json_root["Road_Markings_Color"].as_array();
  if (road_markings_color.size() != 3) {
    std::cerr << autodrive::perception::to_red("[Error] ")
              << "Config json exception, get Road_Markings_Color failed !"
              << std::endl;
    return false;
  }
  road_markings_color_ = {road_markings_color[0].as_double(),
                          road_markings_color[1].as_double(),
                          road_markings_color[2].as_double()};

  // get Car_Color
  if (!viewer_json_root.contains("Car_Color")) {
    std::cerr << autodrive::perception::to_red("[Error] ")
              << "Config json content does not contain the "
                 "Car_Color !"
              << std::endl;
    return false;
  }
  boost::json::array car_color = viewer_json_root["Car_Color"].as_array();
  if (car_color.size() != 3) {
    std::cerr << autodrive::perception::to_red("[Error] ")
              << "Config json exception, get Car_Color failed !" << std::endl;
    return false;
  }
  car_color_ = {car_color[0].as_double(), car_color[1].as_double(),
                car_color[2].as_double()};

  // 计算缩放系数
  viewport_zoom_center_.x = (viewport_x_min_ + viewport_x_max_) / 2.0;
  viewport_zoom_center_.y = (viewport_y_min_ + viewport_y_max_) / 2.0;
  viewport_zoom_center_.z = 0;
  float distance = sqrt(pow(viewport_x_max_ - viewport_x_min_, 2) +
                        (viewport_y_max_ - viewport_y_min_, 2));
  viewport_zoom_distance_ = distance * viewport_zoom_distance_scale_;
  return true;
}

void PerceptionVisualizer::display_lane_divider(
    pcl::visualization::PCLVisualizer::Ptr const& viewer, Map const& map,
    double lane_divider_r, double lane_divider_g, double lane_divider_b,
    int viewport_id, bool random_color) noexcept(false) {
  boost::mt19937 color_seed(time(0));
  boost::uniform_01<> color_prod;

  for (size_t i = 0; i < map.lane_divider.size(); i++) {
    auto const& laneDividerLine{map.lane_divider.at(i)};
    if (laneDividerLine.points.size() > 0) {
      point_cloud_ptr_->clear();
      for (size_t j = 0; j < laneDividerLine.points.size(); j++) {
        auto const& point_end{laneDividerLine.points.at(j)};
        point_cloud_ptr_->push_back({point_end.x, point_end.y, point_end.z});
      }
      if (random_color) {
        lane_divider_r = color_prod(color_seed);
        lane_divider_g = color_prod(color_seed);
        lane_divider_b = color_prod(color_seed);
      }
      std::string view_points_id = std::to_string(viewport_id) +
                                   "_lane_divider_points_" + std::to_string(i);
      base_display_cloud(viewer, point_cloud_ptr_, viewer_point_size_,
                         lane_divider_r, lane_divider_g, lane_divider_b,
                         view_points_id, viewport_id);
      if (is_draw_line_) {
        std::string line_id =
            std::to_string(viewport_id) + "_lane_divider_" + std::to_string(i);
        base_display_line(viewer, point_cloud_ptr_, lane_divider_r,
                          lane_divider_g, lane_divider_b, line_id, viewport_id);
      }
    }
  }
}

void PerceptionVisualizer::display_road_edge(
    pcl::visualization::PCLVisualizer::Ptr const& viewer, Map const& map,
    double road_edge_r, double road_edge_g, double road_edge_b, int viewport_id,
    bool random_color) noexcept(false) {
  boost::mt19937 color_seed(time(0));
  boost::uniform_01<> color_prod;

  for (size_t i = 0; i < map.road_edge.size(); i++) {
    auto const& roadEdgeLine{map.road_edge.at(i)};

    if (roadEdgeLine.points.size() > 0) {
      point_cloud_ptr_->clear();
      for (size_t j = 0; j < roadEdgeLine.points.size(); j++) {
        auto const& point{roadEdgeLine.points.at(j)};
        point_cloud_ptr_->push_back({point.x, point.y, point.z});
      }
      if (random_color) {
        road_edge_r = color_prod(color_seed);
        road_edge_g = color_prod(color_seed);
        road_edge_b = color_prod(color_seed);
      }
      std::string view_points_id = std::to_string(viewport_id) +
                                   "_road_edge_points_" + std::to_string(i);
      base_display_cloud(viewer, point_cloud_ptr_, viewer_point_size_,
                         road_edge_r, road_edge_g, road_edge_b, view_points_id,
                         viewport_id);
      if (is_draw_line_) {
        std::string line_id =
            std::to_string(viewport_id) + "_road_edge_" + std::to_string(i);
        base_display_line(viewer, point_cloud_ptr_, road_edge_r, road_edge_g,
                          road_edge_b, line_id, viewport_id);
      }
    }
  }
}

void PerceptionVisualizer::display_lane_divider_joints(
    pcl::visualization::PCLVisualizer::Ptr const& viewer, Map const& map,
    double lane_divider_joints_r, double lane_divider_joints_g,
    double lane_divider_joints_b, int viewport_id,
    bool random_color) noexcept(false) {
  point_cloud_ptr_->clear();
  for (size_t i = 0; i < map.lane_divider_joint.size(); i++) {
    auto const& point{map.lane_divider_joint.at(i).point};
    point_cloud_ptr_->push_back({point.x, point.y, point.z});
  }
  base_display_cloud(
      viewer, point_cloud_ptr_, viewer_keypoint_size_, lane_divider_joints_r,
      lane_divider_joints_g, lane_divider_joints_b,
      std::to_string(viewport_id) + "_lane_divider_joints", viewport_id);
}

void PerceptionVisualizer::display_road_markings(
    pcl::visualization::PCLVisualizer::Ptr const& viewer, Map const& map,
    double road_markings_r, double road_markings_g, double road_markings_b,
    int viewport_id, bool random_color) noexcept(false) {
  boost::mt19937 color_seed(time(0));
  boost::uniform_01<> color_prod;

  for (size_t i = 0; i < map.road_marking.size(); i++) {
    auto const& roadMarking{map.road_marking.at(i)};
    point_cloud_ptr_->clear();
    for (size_t j = 0; j < roadMarking.points.size(); j++) {
      auto const& point{roadMarking.points.at(j)};
      point_cloud_ptr_->push_back({point.x, point.y, point.z});
    }
    if (random_color) {
      road_markings_r = color_prod(color_seed);
      road_markings_g = color_prod(color_seed);
      road_markings_b = color_prod(color_seed);
    }
    std::string view_points_id = std::to_string(viewport_id) +
                                 "_road_markings_points_" + std::to_string(i);
    base_display_cloud(viewer, point_cloud_ptr_, viewer_point_size_,
                       road_markings_r, road_markings_g, road_markings_b,
                       view_points_id, viewport_id);
    if (is_draw_polygon_) {
      if (point_cloud_ptr_->size() >= 3) {
        std::string view_polygon_id =
            std::to_string(viewport_id) + "_road markings_" + std::to_string(i);
        base_display_polygon(viewer, point_cloud_ptr_, road_markings_r,
                             road_markings_g, road_markings_b, view_polygon_id,
                             viewport_id);
      }
    }
  }
}

void PerceptionVisualizer::display_center_lines(
    pcl::visualization::PCLVisualizer::Ptr const& viewer, Map const& map,
    double center_lines_r, double center_lines_g, double center_lines_b,
    int viewport_id, bool random_color) noexcept(false) {
  boost::mt19937 color_seed(time(0));
  boost::uniform_01<> color_prod;

  for (size_t i = 0; i < map.center_line.size(); i++) {
    auto const& centerLine{map.center_line.at(i)};

    if (centerLine.points.size() > 0) {
      point_cloud_ptr_->clear();
      for (size_t j = 0; j < centerLine.points.size(); j++) {
        auto const& point{centerLine.points.at(j)};
        point_cloud_ptr_->push_back({point.x, point.y, point.z});
      }
      if (random_color) {
        center_lines_r = color_prod(color_seed);
        center_lines_g = color_prod(color_seed);
        center_lines_b = color_prod(color_seed);
      }
      std::string view_points_id = std::to_string(viewport_id) +
                                   "_center_lines_points_" + std::to_string(i);
      base_display_cloud(viewer, point_cloud_ptr_, viewer_point_size_,
                         center_lines_r, center_lines_g, center_lines_b,
                         view_points_id, viewport_id);
      if (is_draw_line_) {
        std::string view_line_id = std::to_string(viewport_id) +
                                   "_center_lines_line_" + std::to_string(i);
        base_display_line(viewer, point_cloud_ptr_, center_lines_r,
                          center_lines_g, center_lines_b, view_line_id,
                          viewport_id);
      }
    }
  }
}

template <typename PointT>
void PerceptionVisualizer::base_display_cloud(
    pcl::visualization::PCLVisualizer::Ptr const& viewer,
    pcl::shared_ptr<pcl::PointCloud<PointT>> const pcl_cloud, double pt_size,
    double r, double g, double b, std::string const& cloud_id,
    int viewport_id) noexcept(false) {
  if (viewer->contains(cloud_id)) {
    std::cout << "[" << to_yellow("WARRNING") << "] "
              << "cloud name: " << cloud_id << " is used before,please check"
              << std::endl;
  }
  if (!pcl_cloud->empty()) {
    viewer->addPointCloud<PointT>(pcl_cloud, cloud_id, viewport_id);
    viewer->setPointCloudRenderingProperties(
        pcl::visualization::PCL_VISUALIZER_POINT_SIZE, pt_size, cloud_id,
        viewport_id);
    viewer->setPointCloudRenderingProperties(
        pcl::visualization::PCL_VISUALIZER_COLOR, r, g, b, cloud_id,
        viewport_id);
  }
}

template <typename PointT>
void PerceptionVisualizer::base_display_line(
    pcl::visualization::PCLVisualizer::Ptr const& viewer,
    pcl::shared_ptr<pcl::PointCloud<PointT>> const pcl_cloud, double r,
    double g, double b, std::string const& line_id,
    int viewport_id) noexcept(false) {
  if (viewer->contains(line_id)) {
    std::cout << "[" << to_yellow("WARRNING") << "] "
              << "line name: " << line_id << " is used before,please check"
              << std::endl;
  }
  if (pcl_cloud->size() >= 2) {
    pcl::PointXYZI pclPoint_line_start;
    pcl::PointXYZI pclPoint_line_end;
    for (size_t i = 1; i < pcl_cloud->size(); i++) {
      auto const& point_start{pcl_cloud->points[i - 1]};
      auto const& point_end{pcl_cloud->points[i]};

      pclPoint_line_start.x = point_start.x;
      pclPoint_line_start.y = point_start.y;
      pclPoint_line_start.z = point_start.z;
      pclPoint_line_end.x = point_end.x;
      pclPoint_line_end.y = point_end.y;
      pclPoint_line_end.z = point_end.z;

      std::string line_id_tmp = line_id + "_" + std::to_string(i);
      viewer->addLine(pclPoint_line_start, pclPoint_line_end, r, g, b,
                      line_id_tmp, viewport_id);
    }
  }
}

template <typename PointT>
void PerceptionVisualizer::base_display_polygon(
    pcl::visualization::PCLVisualizer::Ptr const& viewer,
    pcl::shared_ptr<pcl::PointCloud<PointT>> const pcl_cloud, double r,
    double g, double b, std::string const& polygon_id,
    int viewport_id) noexcept(false) {
  if (viewer->contains(polygon_id)) {
    std::cout << "[" << to_yellow("WARRNING") << "] "
              << "polygon name: " << polygon_id
              << " is used before,please check" << std::endl;
  }
  if (pcl_cloud->size() >= 3) {
    viewer->addPolygon<PointT>(pcl_cloud, r, g, b, polygon_id, viewport_id);
  }
}

void PerceptionVisualizer::UpdataZoomInfo() {
  pcl::PointXYZ minPt, maxPt;
  pcl::getMinMax3D(*point_cloud_ptr_, minPt, maxPt);
  Eigen::Vector3f diff = maxPt.getVector3fMap() - minPt.getVector3fMap();

  float distance = diff.norm() * viewport_zoom_distance_scale_;

  if (distance > viewport_zoom_distance_) {
    viewport_zoom_center_.x = (maxPt.x + minPt.x) / 2.0;
    viewport_zoom_center_.y = (maxPt.y + minPt.y) / 2.0;
    viewport_zoom_center_.z = (maxPt.z + minPt.z) / 2.0;
    viewport_zoom_distance_ = distance;
  }
}

}  // namespace perception
}  // namespace autodrive